#include "elog.h"
#include "elog_file.h"
#include "step_motor_driver.h"
#include "uart.h"

#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include <thread>
#include <mutex>
#include <algorithm>

using std::string;

std::mutex motorMutex;

int StepMotorDriver::fd_ = 0;

//解析电机接口返回的数据
void StepMotorReceiveData::parseUartReceiveData(char* buf, int len) {
    char cmd_mode = 'D';
    int motor_id = -1;
    int run_state = -1;
    int run_mode = -1;
    int num_steps = 0;
    int direction = -1;
    char* value = buf;
    const int return_8_len = 8;
    const int return_4_len = 4;

    int reset_dir = -1;
    int reset_mstep = -1;
    int position = 0;
    int distance = 0;

    int limitX;

    int cur_len = len;

    while(cur_len > 0) {
        if(*value == 0x44) {
            if(*(value + return_8_len - 2) == 0x0d && *(value + return_8_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                run_mode = (*(value + 1) & 0x08) >> 3;
                num_steps = ((*(value + 2)) << 24) + ((*(value + 3)) << 16) + ((*(value + 4)) << 8) + (*(value + 5));

                std::for_each(driver_group_.begin(), driver_group_.end(), [&](StepMotorDriver* driver) {
                    if(driver->getMotorId() == motor_id) {
                        driver->updateMotionPulse(num_steps);
                        driver->updateMotionState(STEP_MOTOR_STOPPED);
                    }
                });

                log_i("motor %d stopped, num_steps:%d", motor_id, num_steps);
                value += return_8_len;
                cur_len -= return_8_len;
            }
            else
                break;
        }

        else if(*value == 0x45) {
            if(*(value + return_4_len - 2) == 0x0d && *(value + return_4_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                run_state = (*(value + 1) & 0x0C) >> 2;

                if(0 == run_state) {
                    std::for_each(driver_group_.begin(), driver_group_.end(), [&](StepMotorDriver* driver) {
                        if(driver->getMotorId() == motor_id)
                            driver->updateMotionState(STEP_MOTOR_STOPPED);
                    });

                    // log_i("check motor %d stopped", motor_id);
                }
                else if(1 == run_state || 2 == run_state || 3 == run_state) {
                    std::for_each(driver_group_.begin(), driver_group_.end(), [&](StepMotorDriver* driver) {
                        if(driver->getMotorId() == motor_id)
                            driver->updateMotionState(STEP_MOTOR_RUNNING);
                    });
                }
                else
                    log_i("check motor %d invalid state", motor_id);
                    
                value += return_4_len;
                cur_len -= return_4_len;
            }
            else
                break;
        }

        else if(*value == 0x46) {
            if(*(value + return_4_len - 2) == 0x0d && *(value + return_4_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                direction = (*(value + 1) & 0x04) >> 2;
                value += return_4_len;
                cur_len -= return_4_len;
            }
            else
                break;
        }   

        else if(*value == 0x58) {
            if(*(value + return_4_len - 2) == 0x0d && *(value + return_4_len - 1) == 0x0a) {
                value += return_4_len;
                cur_len -= return_4_len;
            }
            else
                break;
        }

        else if(*value == 0x53) {
            if(*(value + return_4_len - 2) == 0x0d && *(value + return_4_len - 1) == 0x0a) {
                value += return_4_len;
                cur_len -= return_4_len;
            }
            else
                break;
        }

        else if(*value == 0x4C) {
            if(*(value + return_4_len - 2) == 0x0d && *(value + return_4_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                direction = (*(value + 1) & 0x04) >> 2;
                
                StepMotorLimitInfo  limit_info = {STEP_MOTOR_LIMITED, direction};
                std::for_each(driver_group_.begin(), driver_group_.end(), [&](StepMotorDriver* driver) {
                    if(driver->getMotorId() == motor_id)
                        driver->updateLimitInfo(limit_info);
                });
                
                log_i("motor %d direction:%d limited", motor_id, direction);
                value += return_4_len;
                cur_len -= return_4_len;
            }
            else
                break;
        }
        else if (*value == 0x52){
            if (*(value + return_8_len - 2) == 0x0d && *(value + return_8_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                reset_dir = (*(value + 1) & 0x04) >> 2;
                distance = ((*(value + 2)) << 24) + ((*(value + 3)) << 16) + ((*(value + 4)) << 8) + (*(value + 5));

                StepMotorResetInfo  reset_info = {STEP_MOTOR_DONE, reset_dir, distance};
                std::for_each(driver_group_.begin(), driver_group_.end(), [&](StepMotorDriver* driver) {
                    if(driver->getMotorId() == motor_id)
                        driver->updateResetInfo(reset_info);
                });

                log_i("motor %d reset_dir:%d, distance:%d", motor_id, reset_dir, distance);
                value += return_8_len;
                cur_len -= return_8_len;
            }
            else
                break;
        }
        else if (*value == 0x50){
            if (*(value + return_8_len - 2) == 0x0d && *(value + return_8_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                reset_dir = (*(value + 1) & 0x0C) >> 2;
                position = ((*(value + 2)) << 24) + ((*(value + 3)) << 16) + ((*(value + 4)) << 8) + (*(value + 5));
                
                StepMotorAbsolutePositionInfo  absolute_position_info = {STEP_MOTOR_DONE, position};
                std::for_each(driver_group_.begin(), driver_group_.end(), [&](StepMotorDriver* driver) {
                    if(driver->getMotorId() == motor_id) {
                        if (reset_dir == 2)
                            absolute_position_info.absolute_position = -2;
                        driver->updateAbsolutePosition(absolute_position_info);
                    }
                });

                log_i("motor %d reset_dir:%d, absolute position:%d", motor_id, reset_dir, position);
                value += return_8_len;
                cur_len -= return_8_len;
            }
            else 
                break;
        }
        else if (*value == 0x70){
            if (*(value + return_8_len - 2) == 0x0d && *(value + return_8_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                position = ((*(value + 2)) << 24) + ((*(value + 3)) << 16) + ((*(value + 4)) << 8) + (*(value + 5));
                
                StepMotorRelativePositionInfo  relative_position_info = {STEP_MOTOR_DONE, position};
                std::for_each(driver_group_.begin(), driver_group_.end(), [&](StepMotorDriver* driver) {
                    if(driver->getMotorId() == motor_id)
                        driver->updateRelativePosition(relative_position_info);
                });

                log_i("motor %d relative position:%d", motor_id, position);
                value += return_8_len;
                cur_len -= return_8_len;
            }
            else
                break;
        }
        else if (*value == 0x4a){
            if (*(value + return_4_len - 2) == 0x0d && *(value + return_4_len - 1) == 0x0a) {
                motor_id = *(value + 1) & 0x03;
                limitX = (*(value + 1) & 0x0C) >> 2;
                if(1 == motor_id)
                    log_i("Motor1 limit:%d",limitX);
                else if(2 == motor_id)
                    log_i("Motor2 limit:%d",limitX);
                else if(2 == motor_id)
                    log_i("Motor3 limit:%d",limitX);    
                value += return_4_len;
                cur_len -= return_4_len;
            }
            else
                break;
        }
        else if (*value == 0x00) { // 去除返回数据中的00脏数据
            value ++;
            cur_len --;
        }
        else
            break;
    }
}

void StepMotorDriver::uartRecvThread() {
    static char rcv_buf[128] = {0};
    static int len_l = 0;
    int len = 0; 
    StepMotorReceiveData& recv_data = processRecvData();

    while (1) //循环读取数据    
    {   
        len = UART_Recv(fd_, rcv_buf + len_l, sizeof(rcv_buf) - len_l);   
        if(len > 0)    
        {   
            len_l += len;
            if(len_l >= sizeof(rcv_buf)) {
                log_i("receive data missing terminator"); 
                memset(rcv_buf, 0, sizeof(rcv_buf));
                len_l = 0;
                continue;
            }
            
            // printf("rcv_buf length is %d\n", len_l);
            // printf("receive data:");
            // for(int i = 0;i < len_l;i++) {
            //     printf("%02x ", rcv_buf[i]);
            // }
            // printf("\n");

            if(len_l - 2 >= 0) {
                if(rcv_buf[len_l - 2] == 0x0d && rcv_buf[len_l - 1] == 0x0a) {
                    recv_data.parseUartReceiveData(rcv_buf, len_l);
                    memset(rcv_buf, 0, sizeof(rcv_buf));
                    len_l = 0;
                }
            }
        }    
    }      
}

//初始化串口
int StepMotorDriver::initMotor() {
    int err;

    fd_ = UART_Open(fd_, (char*)"/dev/ttyS3"); //打开串口，返回文件描述符
    err = UART_Init(fd_, 921600, 0, 8, 1, 'N'); 

    if(-1 != fd_ || -1 != err) {
        log_i("open uart successfully!");

        std::thread t_uart_recv(uartRecvThread);
        t_uart_recv.detach();
    } else {
        log_i("open uart failed!");
    }

    return err;
}

bool StepMotorDriver::getMotionState() {
    return motion_info_.motion_state;
}

int StepMotorDriver::getMotionPulse() {
    return motion_info_.motion_pulse;
}

bool StepMotorDriver::getLimitState() {
    if (limit_info_.limit_state == STEP_MOTOR_LIMITED) {
        limit_info_.limit_state = STEP_MOTOR_UNLIMITED;
        return STEP_MOTOR_LIMITED;
    } else {
        return STEP_MOTOR_UNLIMITED;
    }
}

bool StepMotorDriver::getLimitState(int direciton) {
    if(limit_info_.direction == direciton) {
        if (limit_info_.limit_state == STEP_MOTOR_LIMITED) {
            limit_info_.limit_state = STEP_MOTOR_UNLIMITED;
            return STEP_MOTOR_LIMITED;
        } else {
            return STEP_MOTOR_UNLIMITED;
        }
    }
    else {
        return STEP_MOTOR_UNLIMITED;
    }
}

int StepMotorDriver::getResetState() {
    if(reset_info_.reset_flag == STEP_MOTOR_DONE) {
        reset_info_.reset_flag = STEP_MOTOR_YET;
        return reset_info_.reset_distance;
    } else {
        return -1;
    }
}

int StepMotorDriver::getRelativePosition() {
    if(relative_position_info_.read_flag == STEP_MOTOR_DONE) {
        relative_position_info_.read_flag = STEP_MOTOR_YET;
        return relative_position_info_.relative_position;
    } else {
        return -1;
    }
}   

int StepMotorDriver::getAbsolutePosition() {
    if(absolute_position_info_.read_flag == STEP_MOTOR_DONE) {
        absolute_position_info_.read_flag = STEP_MOTOR_YET;
        return absolute_position_info_.absolute_position;
    } else {
        return -1;
    }
}   

/**
 * @brief 
 *      run motor function for DWARF, send command to motor driver
 *  
 * @param motor_id_
 *      motor motor_id_: 
 *      1->A, 2->B, 3->C
 * @param mode 
 *      run mode: 
 *      0->pulse mode, 1->continuity mode, 2->astro pulse mode, 3->astro continuity mode
 * @param mStep :
 *      StepMotorDriver subdivision: 
 *      1, 2, 4, 8, 16, 32, 64, 128, 256
 * @param direction :
 *      DWARF initial position
 *      motor 1: 0->left, 1->right
 *      motor 2: 0->up,   1->down
 * @param speed :
 *      how many pulses send to stepper motor in 1s
 *      MIN: 2
 *      MAX: 32000 > (mStep * 1000) ? (mStep * 1000) : 32000;
 * @param pulse :
 *      how many pulses send to stepper motor
 *      MIN: 1
 *      MAX: ?
 * @param acceltep :
 *      acceleration-deceleration step for motor flexible impact
 * 
 */
int StepMotorDriver::run(int mode, int mStep, int direction, int frequency, int pulse, int accelStep) {
    std::lock_guard<std::mutex> lock(motorMutex);

    // log_i("motor_id_:%d, mode:%d, mstep:%d, dir:%d, speed:%d, pulse:%d", motor_id_, mode, mStep, direction, frequency, pulse);

    if(mode == 0 && pulse == 0)
        return 0;

    motion_info_.motion_state = STEP_MOTOR_RUNNING;
    limit_info_.limit_state = STEP_MOTOR_UNLIMITED;
    
    char send_buf[17] = {0};
    int len;

    send_buf[0] = 0x3E;
    send_buf[1] = 0x40 + motor_id_;
    //方向和细分
    send_buf[2] = direction + (mStep << 1);
    //最小速度
    send_buf[3] = 0x00;
    send_buf[4] = 0x02;
    //最大速度
    send_buf[5] = (frequency & 0xFF00) >> 8;
    send_buf[6] = frequency & 0x00FF;
    //加速步数
    send_buf[7] = (accelStep & 0xFF00) >> 8;
    send_buf[8] = accelStep & 0x00FF;
    //运动步数
    send_buf[9] = (pulse & 0xFF000000) >> 24;
    send_buf[10] = (pulse & 0x00FF0000) >> 16;
    send_buf[11] = (pulse & 0x0000FF00) >> 8;
    send_buf[12] = pulse & 0x000000FF;
    //运动模式
    send_buf[13] = mode;
    //校验码
    for(int i = 1; i < 14;i++)
        send_buf[14] += send_buf[i];

    send_buf[14] = send_buf[14] & 0xFF;
    send_buf[15] = 0x0D;
    send_buf[16] = 0x0A; 

    // log_i("run motor send data = %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
    //         send_buf[0],send_buf[1],send_buf[2],send_buf[3],send_buf[4],send_buf[5],
    //         send_buf[6],send_buf[7],send_buf[8],send_buf[9],send_buf[10],send_buf[11],
    //         send_buf[12], send_buf[13], send_buf[14], send_buf[15], send_buf[16]);
    len = UART_Send(fd_, send_buf, 17);
    if(len < 0) {
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    } 
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}

int StepMotorDriver::stop(int stopMode, int keepTorque, int decelStep) {
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("stop motor %d state = %d", motor_id_, getMotorState(motor_id_));

    char send_buf[8] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x44;
    //电机编号、停转方式、是否保持力矩
    send_buf[2] = (motor_id_ & 0x03) + (stopMode << 2) + (keepTorque << 3);
    //减速步数
    send_buf[3] = (decelStep & 0xFF00) >> 8;
    send_buf[4] = decelStep & 0x000F;
    //校验码
    for(int i = 1; i < 5;i++)
        send_buf[5] += send_buf[i];

    send_buf[5] = send_buf[5] & 0xFF;
    send_buf[6] = 0x0D;
    send_buf[7] = 0x0A; 

    // log_i("stop motor send data = %02x %02x %02x %02x %02x %02x %02x %02x", send_buf[0],send_buf[1],send_buf[2],
    //             send_buf[3],send_buf[4],send_buf[5],send_buf[6],send_buf[7]);
    len = UART_Send(fd_, send_buf, 8);
    // log_i("send stop motor done");
    if(len < 0) {
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }  
    usleep(2 * 1000);   // 等待电机开始执行指令
        
    return 0;
}

int StepMotorDriver::checkMotionState() {
    std::lock_guard<std::mutex> lock(motorMutex);

    char send_buf[6] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x45; 
    send_buf[2] = motor_id_ & 0x03;
    //校验码
    for(int i = 1; i < 3; i++)
        send_buf[3] += send_buf[i];
    send_buf[3] = send_buf[3] & 0xFF;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    // log_i("checkMotorState send data = %02x %02x %02x %02x %02x %02x", send_buf[0],send_buf[1],send_buf[2],
    //             send_buf[3],send_buf[4],send_buf[5]);
    len = UART_Send(fd_, send_buf, 6);
    if(len < 0) {
        log_i("send data failed!");
        // send_result(motor_id_, INTERFACE_MOTOR_START, -RESULT_MOTOR_SEND_FAILED);
        // return -RESULT_MOTOR_SEND_FAILED;
    }
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}

int StepMotorDriver::setDirection(int direction) {
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("set direction %d state = %d", motor_id_, getMotorState(motor_id_));
        
    char send_buf[6] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x46; 
    send_buf[2] = (motor_id_ & 0x03) + (direction << 2);
    //校验码
    for(int i = 1; i < 3;i++)
        send_buf[3] += send_buf[i];
    send_buf[3] = send_buf[3] & 0xFF;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    // log_i("setDirection send data = %02x %02x %02x %02x %02x %02x", send_buf[0],send_buf[1],send_buf[2],
    //             send_buf[3],send_buf[4],send_buf[5]);
    // log_i("dir:%d", direction);
    len = UART_Send(fd_, send_buf, 6);
    if(len < 0) {
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }   
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}

int StepMotorDriver::setMStep(int mStep) {
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("set mStep %d state = %d", motor_id_, getMotorState(motor_id_));

    char send_buf[6] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x58;
    //电机编号、细分
    send_buf[2] = (motor_id_ & 0x03) + (mStep << 2);
    //校验码
    for(int i = 1; i < 3;i++)
        send_buf[3] += send_buf[i];
    send_buf[3] = send_buf[3] & 0xFF;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    // log_i("setMStep send data = %02x %02x %02x %02x %02x %02x", send_buf[0],send_buf[1],send_buf[2],
    //             send_buf[3],send_buf[4],send_buf[5]);
    len = UART_Send(fd_, send_buf, 6);
    if(len < 0) {
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }  
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}

int StepMotorDriver::setFrequency(int trend, int frequency, int accelStep) {
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("set speed %d state = %d", motor_id_, getMotorState(motor_id_));

    char send_buf[10] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x53;
    //电机编号、速度变化趋势 
    send_buf[2] = (motor_id_ & 0x03) + (trend << 2);
    //目标速度
    send_buf[3] = (frequency & 0xFF00) >> 8;
    send_buf[4] = frequency & 0x00FF;
    //加速步数
    send_buf[5] = (accelStep & 0xFF00) >> 8;
    send_buf[6] = accelStep & 0x00FF;
    //校验码
    for(int i = 1; i < 7;i++)
        send_buf[7] += send_buf[i];
    send_buf[7] = send_buf[7] & 0xFF;
    send_buf[8] = 0x0D;
    send_buf[9] = 0x0A;

    // log_i("setSpeed send data = %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", send_buf[0],send_buf[1],send_buf[2],
    //             send_buf[3],send_buf[4],send_buf[5],send_buf[6],send_buf[7],send_buf[8],send_buf[9]);
    // log_i("speed:%d", speed);
    len = UART_Send(fd_, send_buf, 10);
    if(len < 0) {
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}

int StepMotorDriver::reset(int direction) {
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("reset motor %d state = %d", motor_id_, getMotorState(motor_id_));

    motion_info_.motion_state = STEP_MOTOR_RUNNING;
    limit_info_.limit_state = STEP_MOTOR_UNLIMITED;

    char send_buf[6] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x52;
    send_buf[2] = (motor_id_ & 0x03) + (direction << 2);
    //校验码
    for (int i = 1; i < 3; i++)
        send_buf[3] += send_buf[i];
    send_buf[3] = send_buf[3] & 0xFF;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    // log_i("resetMotor send data = %02x %02x %02x %02x %02x %02x", send_buf[0], send_buf[1], send_buf[2],
    //         send_buf[3], send_buf[4], send_buf[5]);
    len = UART_Send(fd_, send_buf, 6);
    if (len < 0) {
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}

int StepMotorDriver::readAbsolutePosition(int mStep) {
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("read position %d state = %d", motor_id_, getMotorState(motor_id_));

    char send_buf[6] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x50;
    //电机编号、细分
    send_buf[2] = (motor_id_ & 0x03) + (mStep << 2);
    //校验码
    for (int i = 1; i < 3; i++)
        send_buf[3] += send_buf[i];
    send_buf[3] = send_buf[3] & 0xFF;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    // log_i("readMotor send data = %02x %02x %02x %02x %02x %02x", send_buf[0], send_buf[1], send_buf[2],
    //         send_buf[3], send_buf[4], send_buf[5]);
    len = UART_Send(fd_, send_buf, 6);
    if (len < 0){
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}

int StepMotorDriver::readRelativePosition() {
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("read relative position %d state = %d", motor_id_, getMotorState(motor_id_));

    char send_buf[6] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x70;
    //电机编号、细分
    send_buf[2] = motor_id_;
    //校验码
    for (int i = 1; i < 3; i++)
        send_buf[3] += send_buf[i];
    send_buf[3] = send_buf[3] & 0xFF;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    // log_i("readMotor send data = %02x %02x %02x %02x %02x %02x", send_buf[0], send_buf[1], send_buf[2],
    //         send_buf[3], send_buf[4], send_buf[5]);
    len = UART_Send(fd_, send_buf, 6);
    if (len < 0){
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}


//禁用指定限位 limitSwitchX 1：禁用限位1 2：禁用限位2 3：禁用限位1+2
int StepMotorDriver::disableLimit(int limitSwitchX){
    std::lock_guard<std::mutex> lock(motorMutex);
    // log_i("disable limit %d state = %d", motor_id_, getMotorState(motor_id_));

    char send_buf[6] = {0};
    int len;

    send_buf[0] = 0x3E;
    //指令类型
    send_buf[1] = 0x4a;
    //电机编号、禁用限位号
    send_buf[2] = (motor_id_ & 0x03) + (limitSwitchX << 2);
    //校验码
    for (int i = 1; i < 3; i++)
        send_buf[3] += send_buf[i];
    send_buf[3] = send_buf[3] & 0xFF;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    // log_i("readMotor send data = %02x %02x %02x %02x %02x %02x", send_buf[0], send_buf[1], send_buf[2],
    //         send_buf[3], send_buf[4], send_buf[5]);
    len = UART_Send(fd_, send_buf, 6);
    if (len < 0){
        log_i("send data failed!");
        // return -RESULT_MOTOR_SEND_FAILED;
    }
    usleep(2 * 1000);   // 等待电机开始执行指令

    return 0;
}
